#ifndef PLATE_HPP
#define PLATE_HPP

#include <opencv2/opencv.hpp>
#include <vector>

class Plate
{
public:
    //all in mm
    double x;
    double y;
    double height;
    double normalAngle;

    unsigned int number;


    //pixel coordinate from plate center
    std::vector<cv::Point2f> vertex;
    cv::Mat image;
    double pixelPermm;


public:
    Plate()
        : number(67)
        , pixelPermm(1)
        , x(0)
        , y(0)
        , height(2000+200-1095-743)
        , normalAngle(0)
    {
        vertex.push_back(cv::Point2f(186,151));

        vertex.push_back(cv::Point2f(100,177));

        //        vertex.push_back(cv::Point2f(152,151));

        vertex.push_back(cv::Point2f(212,128));

        vertex.push_back(cv::Point2f(334,93));

//        vertex.push_back(cv::Point2f(232,308));

        //                vertex.push_back(cv::Point2f(297,128));

        cv::Mat img1 = cv::imread("/home/u/build-untitled-Desktop-Release/67r.png", CV_LOAD_IMAGE_UNCHANGED);

        // Convert to double, 3 channels
        img1.convertTo(image, CV_64F);
    }


    static cv::Vec3d pt2rayf(cv::Vec2d pt, double angle, const cv::Mat &K, const cv::Mat &D)
    {
        cv::Mat R;
        cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
        rv.at<double>(0)=angle;
        cv::Rodrigues(rv, R);

        std::vector<cv::Vec2d> inl(1, pt);
        std::vector<cv::Vec2d> outl;
        cv::fisheye::undistortPoints(inl,outl, K, D, R);

        std::cout<<"outl="<<outl[0]<<"\n"<<std::flush;

        return cv::Vec3d(outl[0][0], outl[0][1], 1);
    }


    static cv::Vec3d pt2ray(cv::Vec2d pt, double angle, const cv::Mat &K, const cv::Mat &D)
    {
        cv::Mat R;
        cv::Mat rv(3, 1, CV_64FC1, cv::Scalar(0));
        rv.at<double>(0)=angle;
        cv::Rodrigues(rv, R);

        std::vector<cv::Vec2d> inl(1, pt);
        std::vector<cv::Vec2d> outl;
        cv::undistortPoints(inl,outl, K, D, R);

        return cv::Vec3d(outl[0][0], outl[0][1], 1);
    }


    static cv::Vec3d ray2center(cv::Vec3d ray, double yheight)
    {

        std::cout<<"yheight="<<yheight<<"\n"<<std::flush;

        return (yheight/ray[1])*ray;
    }


    cv::Vec3d Deproject(cv::Point2f pixel, size_t index, const cv::Mat &K, const cv::Mat &D, double angle)
    {
        return ray2center(pt2ray(cv::Vec2d(pixel.x, pixel.y), angle, K, D), height+(image.rows/2-vertex[index].y)/pixelPermm);
    }


    cv::Vec3d DeprojectF(cv::Point2f pixel, size_t index, const cv::Mat &K, const cv::Mat &D, double angle)
    {
        return ray2center(pt2rayf(cv::Vec2d(pixel.x, pixel.y), angle, K, D), height+(image.rows/2-vertex[index].y)/pixelPermm);
    }

    static cv::Matx22d R2D(double rad)
    {
        return cv::Matx22d(cos(rad), -sin(rad), sin(rad), cos(rad));
    }

    cv::Vec2d GetVertexWorld(size_t index)
    {
        double vertexPlateXmm=(vertex[index].x-image.cols/2)/pixelPermm;
        double vertexPlateYmm=0;

        cv::Matx22d R=R2D(normalAngle);

        cv::Vec2d vertexWorldXYmm=cv::Vec2d(x,y)+R*cv::Vec2d(vertexPlateXmm,vertexPlateYmm);

        std::cout<<"vertexWorldXYmm="<<vertexWorldXYmm<<"\n"<<std::flush;
        return vertexWorldXYmm;
    }


    cv::Vec4d GetEudian(cv::Vec2d p1in, cv::Vec2d p1out, cv::Vec2d p2in, cv::Vec2d p2out)
    {
        cv::Vec4d ABCD=cv::Matx44d(p1in[0],p1in[1],1,0,
                -p1in[1],p1in[0],0,1,
                p2in[0],p2in[1],1,0,
                -p2in[1],p2in[0],0,1).inv()*cv::Vec4d(p1out[0],p1out[1],p2out[0],p2out[1]);

        double theta=atan2(ABCD[1],ABCD[0]);
        double a=sqrt(ABCD[0]*ABCD[0]+ABCD[1]*ABCD[1]);

        std::cout<<"a="<<a<<"\n"<<std::flush;

        return cv::Vec4d(ABCD[2],ABCD[3],theta,a);
    }


    cv::Vec3d GetRigid(const std::vector<cv::Vec2d> &pin, const std::vector<cv::Vec2d> &pout)
    {
        cv::Vec2d avgin(0,0);
        for(auto &p:pin)
            avgin+=p;
        avgin*=1.0/pin.size();

        std::vector<cv::Vec2d> pinr(pin.begin(),pin.end());
        for(auto &p:pinr)
            p-=avgin;

        cv::Vec2d avgout(0,0);
        for(auto &p:pout)
            avgout+=p;
        avgout*=1.0/pout.size();

        double y=0;
        double x=0;
        for(size_t i=0;i<pin.size();i++)
        {
            y+=pout[i][0]*pinr[i][1]-pout[i][1]*pinr[i][0];
            x+=pout[i][0]*pinr[i][0]+pout[i][1]*pinr[i][1];
        }

        double theta=atan2(y,x);


        cv::Vec2d o=avgout-R2D(theta)*avgin;


        return cv::Vec3d(o[0],o[1],theta);
    }


    cv::Vec3d Location(cv::Point2f pixel1, size_t index1, cv::Point2f pixel2, size_t index2, const cv::Mat &K, const cv::Mat &D, double angle)
    {
        cv::Vec3d vertex1CameraXYZmm=Deproject(pixel1, index1, K, D, angle);
        cv::Vec3d vertex2CameraXYZmm=Deproject(pixel2, index2, K, D, angle);


        cv::Vec2d vertex1WorldXYmm=GetVertexWorld(index1);
        cv::Vec2d vertex2WorldXYmm=GetVertexWorld(index2);

        cv::Vec4d camera2World=GetEudian(cv::Vec2d(vertex1CameraXYZmm[0], vertex1CameraXYZmm[2]), vertex1WorldXYmm,
                cv::Vec2d(vertex2CameraXYZmm[0], vertex2CameraXYZmm[2]), vertex2WorldXYmm);

        return cv::Vec3d(camera2World[0],camera2World[1], camera2World[2]);

    }


    cv::Vec3d LocationF(cv::Point2f pixel1, size_t index1, cv::Point2f pixel2, size_t index2, const cv::Mat &K, const cv::Mat &D, double angle)
    {
        cv::Vec3d vertex1CameraXYZmm=DeprojectF(pixel1, index1, K, D, angle);
        cv::Vec3d vertex2CameraXYZmm=DeprojectF(pixel2, index2, K, D, angle);


        std::cout<<"vertex1CameraXYZmm="<<vertex1CameraXYZmm<<"\n"<<std::flush;
        std::cout<<"vertex2CameraXYZmm="<<vertex2CameraXYZmm<<"\n"<<std::flush;

        cv::Vec2d vertex1WorldXYmm=GetVertexWorld(index1);
        cv::Vec2d vertex2WorldXYmm=GetVertexWorld(index2);

        cv::Vec4d camera2World=GetEudian(cv::Vec2d(vertex1CameraXYZmm[0], vertex1CameraXYZmm[2]), vertex1WorldXYmm,
                cv::Vec2d(vertex2CameraXYZmm[0], vertex2CameraXYZmm[2]), vertex2WorldXYmm);

        return cv::Vec3d(camera2World[0],camera2World[1], camera2World[2]);

    }


    cv::Vec3d Location(const std::vector<uchar> &status, const std::vector<float> &err, const std::vector<cv::Point2f> &point,
                       const cv::Mat &K, const cv::Mat &D, double angle, float errThreshold=10)
    {
        std::vector<cv::Vec2d> vertexCameraXYmm;
        std::vector<cv::Vec2d> vertexWorldXYmm;

        for(size_t i=0;i<status.size();i++)
        {
            if(status[i]>0 && err[i]<errThreshold)
            {
                cv::Vec3d vertex1CameraXYZmm=Deproject(point[i], i, K, D, angle);
                vertexCameraXYmm.push_back(cv::Vec2d(vertex1CameraXYZmm[0], vertex1CameraXYZmm[2]));
                vertexWorldXYmm.push_back(GetVertexWorld(i));
            }
        }

        if(vertexCameraXYmm.size()<2)
            return cv::Vec3d(0,0,-1);

        return GetRigid(vertexCameraXYmm, vertexWorldXYmm);
    }


    cv::Vec3d LocationF(const std::vector<uchar> &status, const std::vector<float> &err, const std::vector<cv::Point2f> &point,
                       const cv::Mat &K, const cv::Mat &D, double angle, float errThreshold=10)
    {
        std::vector<cv::Vec2d> vertexCameraXYmm;
        std::vector<cv::Vec2d> vertexWorldXYmm;

        for(size_t i=0;i<status.size();i++)
        {
            if(status[i]>0 && err[i]<errThreshold)
            {
                cv::Vec3d vertex1CameraXYZmm=DeprojectF(point[i], i, K, D, angle);
                vertexCameraXYmm.push_back(cv::Vec2d(vertex1CameraXYZmm[0], vertex1CameraXYZmm[2]));
                std::cout<<"vertex1CameraXYZmm="<<vertex1CameraXYZmm<<"\n"<<std::flush;
                vertexWorldXYmm.push_back(GetVertexWorld(i));
            }
        }

        if(vertexCameraXYmm.size()<2)
            return cv::Vec3d(0,0,-1);

        return GetRigid(vertexCameraXYmm, vertexWorldXYmm);
    }



};

#endif // PLATE_HPP

